import processing.serial.*; //<>//

// 传感器数据
float roll, pitch, yaw;
PVector accelerometer = new PVector();
PVector gyroscope = new PVector();
PVector magneticField = new PVector();

// 3D模型
PShape model;
PImage bgImage;

// 串口配置
Serial port;
String[] serialPorts;
int selectedPort = 0;
boolean printSerial = false;

void setup() {
  size(1024, 800, P3D);
  frameRate(60);
  
  // 加载资源
  bgImage = loadImage("background.png");
  model = loadShape("biplane.obj"); // 确保使用标准OBJ格式
  model.scale(30);
  
  // 初始化串口
  serialPorts = Serial.list();
  if(serialPorts.length > 0) connectSerial(serialPorts[0]);
}

void draw() {
  background(bgImage);
  
  // 3D场景设置
  pushMatrix();
  translate(width/2, height/2, 0);
  lights();
  
  // 应用旋转
  rotateX(radians(pitch));
  rotateY(radians(yaw));
  rotateZ(radians(roll));
  
  // 绘制模型
  shape(model);
  popMatrix();
  
  // 显示数据
  displaySensorData();
}

void serialEvent(Serial p) {
  try {
    String rawData = p.readStringUntil('\n').trim();
    if(printSerial) println(rawData);
    
    String[] parts = split(rawData, ' ');
    if(parts.length >= 4) {
      switch(parts[0]) {
        case "Or:": // 欧拉角格式：Or: yaw pitch roll
          yaw = float(parts[1]);
          pitch = float(parts[2]);
          roll = float(parts[3]);
          break;
        case "Accel:":
          accelerometer.set(float(parts[1]), float(parts[2]), float(parts[3]));
          break;
        case "Gyro:":
          gyroscope.set(float(parts[1]), float(parts[2]), float(parts[3]));
          break;
        case "Mag:":
          magneticField.set(float(parts[1]), float(parts[2]), float(parts[3]));
          break;
      }
    }
  } catch(Exception e) {
    println("Serial Error: " + e.getMessage());
  }
}

void displaySensorData() {
  fill(0, 255, 0);
  textSize(16);
  textAlign(LEFT, TOP);
  String data = "Accelerometer(g): " 
    + nfp(accelerometer.x,1,2) + ", " 
    + nfp(accelerometer.y,1,2) + ", " 
    + nfp(accelerometer.z,1,2) + "\n"
    + "Gyroscope(deg/s): " 
    + nfp(gyroscope.x,1,2) + ", "
    + nfp(gyroscope.y,1,2) + ", " 
    + nfp(gyroscope.z,1,2) + "\n"
    + "Orientation: \n"
    + "Yaw: " + nfp(yaw,1,1) + "°\n"
    + "Pitch: " + nfp(pitch,1,1) + "°\n"
    + "Roll: " + nfp(roll,1,1) + "°";
  text(data, 20, 20);
}

void connectSerial(String portName) {
  if(port != null) port.stop();
  try {
    port = new Serial(this, portName, 115200);
    port.bufferUntil('\n');
    println("Connected to: " + portName);
  } catch(Exception e) {
    println("Connection failed: " + e.getMessage());
  }
}

void keyPressed() {
  // 切换串口
  if(key == ' ') {
    selectedPort = (selectedPort + 1) % serialPorts.length;
    connectSerial(serialPorts[selectedPort]);
  }
  // 切换调试输出
  if(key == 'P' || key == 'p') printSerial = !printSerial;
  // 重置视角
  if(key == 'R' || key == 'r') {
    yaw = pitch = roll = 0;
  }
}
